from typing import Tuple

import numpy as np


def camera_from_world_transform(d: float = 1.0) -> np.ndarray:
    """Define a transformation matrix in homogeneous coordinates that
    transforms coordinates from world space to camera space, according
    to the coordinate systems in Question 1.


    Args:
        d (float, optional): Total distance of displacement between world and camera
            origins. Will always be greater than or equal to zero. Defaults to 1.0.

    Returns:
        T (np.ndarray): Left-hand transformation matrix, such that c = Tw
            for world coordinate w and camera coordinate c as column vectors.
            Shape = (4,4) where 4 means 3D+1 for homogeneous.
    """
    T = np.eye(4)
    # YOUR CODE HERE 
    theta = np.radians(45+90)

    # 平移矩阵
    T_2 = np.array([
        [1, 0, 0, -d/np.sqrt(2)],
        [0, 1, 0, 0],
        [0, 0, 1, -d/np.sqrt(2)],
        [0, 0, 0, 1]
    ])

    T_1 = np.array([
        [np.cos(theta), 0, np.sin(theta), 0],
        [0, 1, 0, 0],
        [-np.sin(theta), 0, np.cos(theta), 0],
        [0, 0, 0, 1]
    ])

    T = T_1 @ T_2
    
    # END YOUR CODE
    assert T.shape == (4, 4)
    return T


def apply_transform(T: np.ndarray, points: np.ndarray) -> Tuple[np.ndarray]:
    """Apply a transformation matrix to a set of points.

    Hint: You'll want to first convert all of the points to homogeneous coordinates.
    Each point in the (3,N) shape edges is a length 3 vector for x, y, and z, so
    appending a 1 after z to each point will make this homogeneous coordinates.

    You shouldn't need any loops for this function.

    Args:
        T (np.ndarray):
            Left-hand transformation matrix, such that c = Tw
                for world coordinate w and camera coordinate c as column vectors.
            Shape = (4,4) where 4 means 3D+1 for homogeneous.
        points (np.ndarray):
            Shape = (3,N) where 3 means 3D and N is the number of points to transform.

    Returns:
        points_transformed (np.ndarray):
            Transformed points.
            Shape = (3,N) where 3 means 3D and N is the number of points.
    """
    N = points.shape[1]
    assert points.shape == (3, N)

    # You'll replace this!
    points_transformed = np.zeros((3, N))

    # YOUR CODE HERE
    # 将points转换为齐次坐标
    points_homogeneous = np.vstack((points, np.ones((1, N))))
    # 进行矩阵乘法
    points_transformed_homogeneous = T @ points_homogeneous
    # 取前3行
    points_transformed = points_transformed_homogeneous[:3, :]

    # END YOUR CODE

    assert points_transformed.shape == (3, N)
    return points_transformed


def intersection_from_lines(
    a_0: np.ndarray, a_1: np.ndarray, b_0: np.ndarray, b_1: np.ndarray
) -> np.ndarray:
    """Find the intersection of two lines (infinite length), each defined by a
    pair of points.

    Args:
        a_0 (np.ndarray): First point of first line; shape `(2,)`.
        a_1 (np.ndarray): Second point of first line; shape `(2,)`.
        b_0 (np.ndarray): First point of second line; shape `(2,)`.
        b_1 (np.ndarray): Second point of second line; shape `(2,)`.

    Returns:
        np.ndarray: the intersection of the two lines definied by (a0, a1)
                    and (b0, b1).
    """
    # Validate inputs
    assert a_0.shape == a_1.shape == b_0.shape == b_1.shape == (2,)
    assert a_0.dtype == a_1.dtype == b_0.dtype == b_1.dtype == float

    # Intersection point between lines
    out = np.zeros(2)

    # YOUR CODE HERE
    # 延长边，找到交点 运用numpy
    # 计算斜率
    a_slope = (a_1[1] - a_0[1]) / (a_1[0] - a_0[0])
    b_slope = (b_1[1] - b_0[1]) / (b_1[0] - b_0[0])
    # 计算截距
    a_intercept = a_0[1] - a_slope * a_0[0]
    b_intercept = b_0[1] - b_slope * b_0[0]
    # 计算交点
    x = (b_intercept - a_intercept) / (a_slope - b_slope)
    y = a_slope * x + a_intercept
    out[0] = x
    out[1] = y
    # END YOUR CODE

    assert out.shape == (2,)
    assert out.dtype == float

    return out


def optical_center_from_vanishing_points(
    v0: np.ndarray, v1: np.ndarray, v2: np.ndarray
) -> np.ndarray:
    """Compute the optical center of our camera intrinsics from three vanishing
    points corresponding to mutually orthogonal directions.

    Hints:
    - Your `intersection_from_lines()` implementation might be helpful here.
    - It might be worth reviewing vector projection with dot products.

    Args:
        v0 (np.ndarray): Vanishing point in image space; shape `(2,)`.
        v1 (np.ndarray): Vanishing point in image space; shape `(2,)`.
        v2 (np.ndarray): Vanishing point in image space; shape `(2,)`.

    Returns:
        np.ndarray: Optical center; shape `(2,)`.
    """
    assert v0.shape == v1.shape == v2.shape == (2,), "Wrong shape!"

    optical_center = np.zeros(2)

    # YOUR CODE HERE
    def line_through_orthogonal_to(p: np.ndarray, q: np.ndarray, r: np.ndarray):
        # Compute slope of line through q and r
        dx = r[0] - q[0]
        dy = r[1] - q[1]
        if dy == 0:
            slope = 0
        elif dx == 0:
            slope = np.inf
        else:
            slope = dx / (q[1] - r[1])  # from textbook: (x_s - x_r)/(y_r - y_s)
        # Line through p with this slope
        if np.isinf(slope):
            return None, p[0]  # vertical line
        else:
            intercept = p[1] - slope * p[0]
            return slope, intercept

    # Line L1: from v0, orthogonal to v1-v2
    m1, b1 = line_through_orthogonal_to(v0, v1, v2)

    # Line L2: from v1, orthogonal to v0-v2
    m2, b2 = line_through_orthogonal_to(v1, v0, v2)

    # Compute intersection of L1 and L2
    if m1 is None:  # L1 is vertical
        x = b1
        y = m2 * x + b2
    elif m2 is None:  # L2 is vertical
        x = b2
        y = m1 * x + b1
    else:
        x = (b2 - b1) / (m1 - m2)
        y = m1 * x + b1

    optical_center = np.array([x, y])
    # END YOUR CODE

    assert optical_center.shape == (2,)
    return optical_center


def focal_length_from_two_vanishing_points(
    v0: np.ndarray, v1: np.ndarray, optical_center: np.ndarray
) -> np.ndarray:
    """Compute focal length of camera, from two vanishing points and the
    calibrated optical center.

    Args:
        v0 (np.ndarray): Vanishing point in image space; shape `(2,)`.
        v1 (np.ndarray): Vanishing point in image space; shape `(2,)`.
        optical_center (np.ndarray): Calibrated optical center; shape `(2,)`.

    Returns:
        float: Calibrated focal length.
    """
    assert v0.shape == v1.shape == optical_center.shape == (2,), "Wrong shape!"

    f = None

    # YOUR CODE HERE
    # 向量从光心到消失点
    dx0, dy0 = v0 - optical_center
    dx1, dy1 = v1 - optical_center

    # 根据正交关系 (v0−c)·(v1−c) + f^2 = 0
    dot = dx0 * dx1 + dy0 * dy1
    f_sq = -dot

    # 防止数值误差导致负数
    f = np.sqrt(f_sq) if f_sq > 0 else 0.0
    # END YOUR CODE

    return float(f)


def physical_focal_length_from_calibration(
    f: float, sensor_diagonal_mm: float, image_diagonal_pixels: float
) -> float:
    """Compute the physical focal length of our camera, in millimeters.

    Args:
        f (float): Calibrated focal length, using pixel units.
        sensor_diagonal_mm (float): Length across the diagonal of our camera
            sensor, in millimeters.
        image_diagonal_pixels (float): Length across the diagonal of the
            calibration image, in pixels.

    Returns:
        float: Calibrated focal length, in millimeters.
    """
    f_mm = None

    # YOUR CODE HERE
    # 计算物理焦距
    f_mm = f * (sensor_diagonal_mm / image_diagonal_pixels)
    # END YOUR CODE

    return f_mm
